#include "XPT2046.h"
#include <algorithm>
extern USART cout;
constexpr uint8_t CMD_X = 0xD0;
constexpr uint8_t CMD_Y = 0x90;

template<uint32_t Ignore>
uint16_t XPT2046::Filter(uint16_t* Arr, uint32_t Len) const
{
	uint32_t ret = 0;
	std::sort(Arr, Arr + Len); //升序排序
	for (uint32_t i = Ignore; i < Len - Ignore; i++)
		ret += Arr[i];
	return ret / (Len - 2 * Ignore);
}

uint16_t XPT2046::GetAverage(uint8_t XY) const
{
	uint16_t buf[40];
	for (auto& i : buf)
	{
		Port.Read(XY, &i, 2);
		i = __REV16(i);//Read读取为字节顺序,STM32内存为小端序
		i >>= 3;
	}
	return this->Filter<1>(buf, sizeof(buf) / sizeof(*buf));
}

uint16_t XPT2046::Get(uint16_t& X, uint16_t& Y) const
{
	uint16_t x1, x2, y1, y2, err1, err2;

	x1 = this->GetAverage(CMD_X);
	y1 = this->GetAverage(CMD_Y);

	x2 = this->GetAverage(CMD_X);
	y2 = this->GetAverage(CMD_Y);

	err1 = std::labs(x1 - x2);
	err2 = std::labs(y1 - y2);

	if ((err1 < 50) and (err2 < 50))
	{
		X = (x1 + x2) / 2;
		Y = (y1 + y2) / 2;
		return 1;
	}
	else
		return 0;
}
